#define _CRT_SECURE_NO_WARNINGS

#ifndef TRAJECTORY_PREDICTION_HPP
#define TRAJECTORY_PREDICTION_HPP

#include <cstdio>
#include <stdlib.h>
#include <stdint.h>
#include <stdio.h>

#include <opencv2/opencv.hpp>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

using namespace std;
using namespace cv;

#define DETECT_PLANE -0.6 // 轨迹预测的终点平面的高度
#define PRED_THRES 10 // 开始轨迹预测的阈值，轨迹点数量大于该值开始预测
#define CLIP_NUM 8 // 只取已有轨迹点的后半段的数量

extern int HitFlag;

typedef pcl::PointXYZ 			PointType;
typedef pcl::PointCloud<PointType> 	PointCloud;
typedef PointCloud::Ptr 		pPointCloud;

struct OutputValue  //输出数据结构体
{
    float delta_time = 0;   // 到指定平面的剩余时间
    float out_time = 0;     // 收到击球指令后经过的时间
    float end_speed = 0;    // 到终点的合速度
    float end_x = 0;        // 到终点的x坐标
    float end_z = 0.178;        // 到终点的z坐标
    float end_yaw = 0;      // 到终点的偏航角
    float end_pitch = 0;    // 到终点的俯仰角
    float max_y = 0;        // 轨迹的最高点y坐标
    float end_speed_x = 0;      // 到终点的x方向分速度
    float end_speed_y = 0;      // 到终点的y方向分速度
    float end_speed_z = 0;      // 到终点的z方向分速度
    float current_x = 0;        // 当前点的x坐标
    float current_y = 0;        // 当前点的y坐标
    float current_z = 0.178;        // 当前点的z坐标
    float current_speed_x = 0;  // 当前点的x方向分速度
    float current_speed_y = 0;  // 当前点的y方向分速度
    float current_speed_z = 0;  // 当前点的z方向分速度
    float current_yaw = 0;      // 当前点的偏航角
    float current_pitch = 0;    // 当前点的俯仰角
    // float result_x_2 = 0;
    float result_x_1 = 0;
    float result_x_0 = 0;
    float result_y_2 = 0;
    float result_y_1 = 0;
    float result_y_0 = 0;
    // float result_z_2 = 0;
    float result_z_1 = 0;
    float result_z_0 = 0;

    inline void clear(){
        delta_time = 0;
        out_time = 0;
        end_speed = 0;
        end_x = 0;
        end_z = 0.178;
        end_yaw = 0;
        end_pitch = 0;
        max_y = 0;
        end_speed_x = 0;
        end_speed_y = 0;
        end_speed_z = 0;
        current_x = 0;
        current_y = 0;
        current_z = 0.178;
        current_speed_x = 0;
        current_speed_y = 0;
        current_speed_z = 0;
        current_yaw = 0;
        current_pitch = 0;
        // result_x_2 = 0;
        result_x_1 = 0;
        result_x_0 = 0;
        result_y_2 = 0;
        result_y_1 = 0;
        result_y_0 = 0;
        // result_z_2 = 0;
        result_z_1 = 0;
        result_z_0 = 0;
    };
};

class TraPredict
{
public:
    TraPredict();
    ~TraPredict();
    bool Ball_trajectory(const PointType center_point, const timeval& time, Eigen::VectorXf& result_x, Eigen::VectorXf& result_y, Eigen::VectorXf& result_z);
    bool Output_process(const timeval& time, const Eigen::VectorXf& result_x, const Eigen::VectorXf& result_y, const Eigen::VectorXf& result_z, OutputValue& output);
    pPointCloud tra_point_cloud;    // 所有球心组成的轨迹点云
    std::vector<float> tra_time_sec;    // 所有时间戳的容器
    PointType filted_point;     // 将原始球心点做去无效点处理后得到的点
    Eigen::VectorXf result_x;   // 拟合出的轨迹方程的系数
    Eigen::VectorXf result_y;
    Eigen::VectorXf result_z;
    OutputValue tra_output;     //最终输出的数据
    int get_detect_flag() {return detect_flag;}

private:
    
    std::vector<float> time_sec;    // 每个球心位置对应的时间
    std::vector<Point3f> ball_speed;     // 每个点对应的速度
    std::vector<Point3f> ball_acceleration;      // 每个点对应的加速度
    vector<float> ball_tra_x;   // 单个轨迹内的xyz坐标
    vector<float> ball_tra_y;
    vector<float> ball_tra_z;
    float start_time;   // 单个轨迹起点的时间戳
    int detect_flag;    // 检测到轨迹的标志位
    

    int start_flag(pPointCloud tra_point_cloud, vector<float> tra_time_sec);
    int end_flag(pPointCloud tra_point_cloud, vector<float> tra_time_sec);
    Eigen::VectorXf FitterLeastSquareMethod(vector<float> &X, vector<float> &Y, uint8_t orders);
};





#endif